/****************************************************/
// MSP432P401R
// 配置Keil独立空工程 (已将ti固件库打包至工程)
// Keil工程已配置开启FPU
// Bilibili：m-RNA
// E-mail:m-RNA@qq.com
// 创建日期:2021/9/28
/****************************************************/
#include "oled.h"
#include "sysinit.h"
#include "usart.h"
#include "delay.h"
#include "led.h"
#include "tim32.h"
#include <stdint.h>
#include <stdbool.h>
#include "driverlib.h"
#include "key.h"
#include "exti.h"
#include "timA.h"
#include "usart2.h"
#include "openmv.h"
#include "uart3.h"
#include "receive.h"
void mode1(void);
void mode2(void);
void mode3(void);
void control(void);
void GREEN_RED(void);
void mode4(void);
void evadible(void);
u8 second=0;
u8 thrid=0;
//1800向左
//1200向右
//左边小于0
//右边大于0
//Tim32_0_Int_Init(3000000 - 1, TIMER32_PRESCALER_16); //48M/16=3000000  3000000/3000000=1hz=1s
 u8 stop_time=0;
 int reverse_time=40;
 int reverse_time1=120;
u8 stop_stop=43;
u8 stop_stop2=53;
u8 stop_stop3=30;
u8 flag33=0;
	static u8 yx;
	static u8 ii;
	static	u8 yyy=0;
	static	u8 yy=0;
	static	u8 xx=0;
	static	u8 xxx=0;
	float distan;
int main(void)
{  
	  u8 pattern,len;
    SysInit();         // 第3讲 时钟配置
    uart_init(9600); // 第7讲 串口配置
	  uart2_init(115200); // 第7讲 串口配置
		uart3_init(115200); // 第7讲 串口配置
    delay_init();      // 第4讲 滴答延时
    OLED_Init();
    LED_Init();
    KEY_Init(0);
	  check_line();
		TimA2_Cap_Init(); 
	  Tim32_0_Int_Init(15000-1,TIMER32_PRESCALER_16);
	  //需要进行超声波的初始化端口

	  moto_init();     //电机端口初始化
	  TimA0_PWM_Init(20000-1,48);
	  TimA1_PWM_Init(2000-1,24);  //电机pwm及舵机pwm波输出
    MAP_Interrupt_enableMaster(); // 开启总中断
	  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);//	
	  delay_ms(3000);
// 
		UL=1;
		delay_us(10);
		UL=0;

    while (1)
    {		               
	      
			LED_G_Tog();
				if(USART_RX_STA&0x8000)
				{
						len=USART_RX_STA&0x3fff;                     
					  pattern=USART_RX_BUF[0];
						USART_RX_STA=0;		
            OLED_ShowString(0,0,USART_RX_BUF,16,1);
						OLED_ShowNum(0,16,pattern,3,16);
			      OLED_Refresh_Gram();						
				}
//				evadible(); 
//				  GREEN_RED();
		OLED_ShowNum(16,48,green_go,3,16);
			OLED_ShowNum(16,48,red_stop,3,16);
   OLED_Refresh_Gram();					
				
				 if(pattern==65)  mode1();   //两米停
				 if(pattern==66)  mode2();   //两圈停
				 if(pattern==67)  mode3();   //侧方停车
				 if(pattern==68)  mode4();   //倒车入库
				 if(pattern==69)  evadible();   //避障
				 if(pattern==70)  GREEN_RED();   //红绿灯  

    }
}



////////////////////////前进两米停下来////////////////////
void mode1(void)   
{
     	  for(yyy=0;yyy<25;yyy++)
		{
			
			if(line_offset>0&&line_offset<35)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);
			if(line_offset>35&&line_offset<130)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);
			if(line_offset>130)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
	    
            
			OLED_ShowDecimals(0,30,"THEATER",16,theater);
			OLED_ShowDecimals(0,0,"line",16,line_offset);
			OLED_Refresh_Gram();	
	
         go();
		     
		     delay_ms(100);
		}
			
			while(1)
			{
			stop();}
}



///////////////////////// //两圈之后停在起点////////////////////////////////
void mode2(void) 
{

	   
			if(line_offset>0&&line_offset<30)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);
			if(line_offset>30&&line_offset<120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);
			if(line_offset>120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
	    
            
			OLED_ShowDecimals(0,30,"THEATER",16,theater);
			OLED_ShowDecimals(0,0,"line",16,line_offset);
			OLED_Refresh_Gram();	
	
      go();
			
			
			if(theater>55&&theater<130)
			{
				yx+=1;
				for(ii=0;ii<20;ii++)
				{
				 moto1=0,moto2=0;
				 MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,600); //motor
				 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1350);				
				delay_ms(20);
			
				}
				
         	if(yx>=4&&yx<=6)
			{
			   while(1)
				 {
				   stop();
					 OLED_ShowString(0,48,"okok",16,1);
				 OLED_Refresh_Gram();
				 }
			
			}  

			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1430);					
	     go();
			 delay_ms(530);
			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);				
			}
}

///////////////////////////////////////////////穿越门框停车////////////////
void mode3(void)
{

//for(yy=0;yy<28;yy++)
//			{
			MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1100);//		
			
		  go_slow();	
			delay_ms(2660);
//			}
		
			  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 2030);//	
			  go_slow();
				delay_ms(615);
			while(1)
			{
//				MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);//	
//       for(xx=0;xx<95;xx++)
//				{
					MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);//	
				   go_slow();
					delay_ms(2830);
				
//				}
			  stop();
				delay_ms(100);
//	      while(1)
//					{   
//				for(xxx=0;xxx<18;xxx++)
//			{
			MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);//		
			
		  go_slow();	
			delay_ms(1800);
//			}
				while(1){stop();}
//				
//				    }
			}
}
//////////////////////////////////////////////////倒车入库////////////////////////

void mode4(void)
{


			if(line_offset>0&&line_offset<30)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);
			if(line_offset>30&&line_offset<120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);
			if(line_offset>120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
	    
	    
            
			OLED_ShowDecimals(0,30,"THEATER",16,theater);
			OLED_ShowDecimals(0,0,"line",16,line_offset);
			OLED_Refresh_Gram();	
	
      go();
			
			
			if(theater>60&&theater<130)
			{
				yx+=1;
				for(ii=0;ii<20;ii++)
				{
				 moto1=0,moto2=0;
				 MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,600); //motor
				 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1350);				
				delay_ms(20);
			
				}
				if(yx==2)MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1650);		
				
     			if(yx==3||yx>3)
						{
							moto1=0,moto2=1;
		         MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,800);
						 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1100);				
							delay_ms(430);
							stop();
							delay_ms(500);
								//右二向左小幅度     左二向右小幅度
							 while(1)
							 { 
												reverse();
	
										
									if(left2==1) MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);	
									if(left1==1) MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1100);	
									if(right2==1) MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1650);
									if(right1==1) MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1750);


/////////////////////////////////
									if(left1==1&&right2==1)
									{ 
												while(reverse_time1--)
													{
												MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1160);	
												reverse();
												delay_ms(30);
										      }							
									    while(1)
												{
												MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1430);			
                               reverse();delay_ms(1200);   
                      while(1)
											{												
											stop();MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1430);}	}		

									}
////////////////////////////////									
									
										
									if(turn==1)
									{
											while(reverse_time--){
										MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1150);	
										reverse();
										delay_ms(30);
										}
											while(1)
											{
										 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);
												reverse();
												delay_ms(1350);
												while(1){stop();}

											}
										
									}

									
							 }
						
						}
	     MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1430);					
	    go();
			 delay_ms(530);
			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);								

			}

}

/////////////////////////////////红绿灯/////////////////////////////
void GREEN_RED(void)
{
       
	   		if(red_stop==1&&green_go==1)
				{
				while(1){stop(); if(red_stop==0&&green_go==1) break;   }
				}
				
					if(red_stop==1)
				{
				while(1){stop(); if(red_stop==0&&green_go==1) break;  }
				}

	
	
			if(line_offset>0&&line_offset<30)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);
			if(line_offset>30&&line_offset<120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);
			if(line_offset>120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
			OLED_ShowDecimals(0,30,"THEATER",16,theater);
			OLED_ShowDecimals(0,0,"line",16,line_offset);
			OLED_Refresh_Gram();		
      go();
						
			if(theater>55&&theater<130)
			{
				yx+=1;
				for(ii=0;ii<20;ii++)
				{
				 moto1=0,moto2=0;
				 MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,600); //motor
				 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1350);				
				delay_ms(20);		
				}
				
         	if(yx>=4&&yx<=6)
			{
			   while(1)
				 {
				   stop();
					 OLED_ShowString(0,48,"okok",16,1);
				 OLED_Refresh_Gram();
				 }
			
			}  

			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1430);					
	     go();
			 delay_ms(530);
			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);				
			}

}

/////////////////////////模式五s弯避障///////////////////
void evadible(void)
{
     
//			OLED_ShowNum(0,20,temp*170/10000,4,16);//显示测量
//		  OLED_Refresh_Gram();
	    go();
	
      if(distan>20&&distan<30)
			{
				 go_slow();
				 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1800);
				delay_ms(1200);
				while(stop_stop--)
				{
				  go_slow();
				  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1150);
					delay_ms(20);					
					flag33=1;  
				}
								
			}

				 while(	flag33==1)
			 {
	 
	    go();				 
			if(line_offset>0&&line_offset<30)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);
			if(line_offset>30&&line_offset<120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);
			if(line_offset>120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
				 
				 while(second)
				 {
							go();				 
				if(line_offset>0&&line_offset<30)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);
				if(line_offset>30&&line_offset<120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);
				if(line_offset>120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
				 
					 
						 if(distan>20&&distan<30)
						{
									go_slow();
									MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1150);
									delay_ms(1000);
										
										while(stop_stop2--)
								{
									OLED_ShowNum(0,36,stop_stop2,4,16);
									OLED_Refresh_Gram();
									go_slow();
									MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1800);
									delay_ms(20);				
								}
											while(stop_stop3--)
								{
									OLED_ShowNum(0,36,stop_stop2,4,16);
									OLED_Refresh_Gram();
									go_slow();
									MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
									delay_ms(20);				
								}
               thrid=1;
						}
				 
						
						while(thrid)
						{
										go();				 
				if(line_offset>0&&line_offset<30)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1700);
				if(line_offset>30&&line_offset<120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1400);
				if(line_offset>120)  MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1200);
				if(theater>55&&theater<130)
			{
				yx=1;
				if(yx==1)
				{
					 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1330);
					go_slow();
					delay_ms(240);
				while(1)
					{
				stop();}
				
				
				}                     //停车
				else {
				for(ii=0;ii<20;ii++)
				{
				 moto1=0,moto2=0;
				 MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,600); //motor
				 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1350);		 					
				delay_ms(20);			
				}
        
			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);					
	     go_slow();
			 delay_ms(660);
			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);	
				}
				
			}
	
						}
						
				 }
				 
	    
/////////////////////////////////////
	    	if(theater>55&&theater<130)
			{
				for(ii=0;ii<20;ii++)
				{
				 moto1=0,moto2=0;
				 MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,600); //motor
				 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1350);		
         if(left1==1&&left2==1&&right1==1&&right2==1)yx+=1;   					
				delay_ms(20);			
				}

			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);					
	     go_slow();
			 delay_ms(660);
			 MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_2, 1450);	
			second=1;
			}
///////////////////////////////			
			OLED_ShowDecimals(30,30,"yx",3,yx);
			OLED_Refresh_Gram();
    
	     }

}

